// gcc time_cost.c -Wall -lm -o time_cost
#include <stdio.h>
#include <stdint.h>
#include <math.h>

// 每圈200步（360/1.8），16细分，每一分钟60秒
#define RPM (200.0f * 16.0f / 60.0f)

typedef struct step_ctrl_t
{
    int s;     // 加速过程需要的步数
    int i;     // 当前步
    int steps; // 步数
    float vi;  // 当前速度
    float a;   // 加速度（脉冲/s2）
} step_ctrl;

step_ctrl stepper_ctrl[4] = {0};

static void stepper_move(int sm, int steps, float v0, float v, float a)
{
    step_ctrl *p_step_ctrl = &stepper_ctrl[sm];
    p_step_ctrl->vi = v0;
    p_step_ctrl->a = a;
    p_step_ctrl->s = (int)roundf((v * v - v0 * v0) / (2 * a)); // 计算加速过程需要的步数
    p_step_ctrl->i = 0;
    p_step_ctrl->steps = steps;
    //pio_set_irq0_source_enabled(pio0, PIO_INTR_SM0_TXNFULL_LSB + sm, true);
}
static uint32_t stepper_move_calc(step_ctrl *p_step_ctrl)
{
    const float F = 10000000.0f; // PIO频率
    float accel;
    int i = p_step_ctrl->i;
    int s = p_step_ctrl->s;
    int steps = p_step_ctrl->steps;
    float vi = p_step_ctrl->vi;
    float a = p_step_ctrl->a;
    if (i < s && i < steps / 2)
    {
        accel = a;
    }
    else if (i >= steps - s)
    {
        accel = -a;
    }
    else
    {
        accel = 0;
    }
    vi = sqrtf(vi * vi + 2.0f * accel);
    p_step_ctrl->vi = vi;
    p_step_ctrl->i = i + 1;
    return (uint32_t)roundf(F / vi); // 33的来源参考step_ctrl.pio
}
static void calc(int steps, float v0, float v, float a)
{
    stepper_move(0, steps, v0, v, a);
    float totle_time = 0, time = 0;
    step_ctrl *p = &stepper_ctrl[0];
    while (1)
    {
        time = stepper_move_calc(p) / 10.0f;
        totle_time += time;
        printf("%d, %.1fus, %.2fus\n", p->i, time, totle_time);
        if (p->i >= p->steps)
        {
            break;
        }
    }
}

int main(int argc, char **argv)
{
    if (argc != 5)
    {
        printf("Usage: %s steps v0 v a\n", argv[0]);
    }
    else
    {
        float v0, v, a;
        int steps;
        sscanf(argv[1], "%d", &steps);
        sscanf(argv[2], "%f", &v0);
        sscanf(argv[3], "%f", &v);
        sscanf(argv[4], "%f", &a);
        printf("calc time of stepper_move_block(0, %d, %f * RPM, %f * RPM, %f);\n", steps, v0, v, a);
        calc(steps, v0*RPM, v*RPM, a);
    }
}
